con

  _clkmode = xtal1 + pll8x
  _clkfreq = 80_000_000

  'written for the Propeller Backpack board (note a 10 MHz xtal)

  APIN          = 0
  BPIN          = 1
  CPIN          = 2
  DPIN          = 3
  EPIN          = 4
  FPIN          = 5
  GPIN          = 6
  HPIN          = 7

  XBEE_RX       = 30
  XBEE_TX       = 31

  
  
  
  LEFT_MOTOR    = 0
  RIGHT_MOTOR   = 1
  

  XBEE_BAUD     = 9_600
  
  IN_BUFF_SIZE  = 8
  OUT_BUFF_SIZE = 8

  PWM_FREQ        = _clkfreq / 50
  
var

  byte  motor_power[2]
  byte  turbo[2]
  byte  motors_enabled
  byte  in_packet[IN_BUFF_SIZE]

  byte  landing_assist

  

obj

  motor[2]      : "pwmAsm"
  'xb           : "xbee_loader"
  xb            : "FullDuplexSerial"
  
pub init

  'start the motors
  motor[LEFT_MOTOR].start(APIN)
  motor[RIGHT_MOTOR].start(BPIN)

  'set initial period
  motor[LEFT_MOTOR].SetPeriod(PWM_FREQ)
  motor[RIGHT_MOTOR].SetPeriod(PWM_FREQ)

  'make sure the motors are off at startup
  motors_enabled := false
  
  'startup xbee communications
  xb.start(XBEE_RX, XBEE_TX, 0, XBEE_BAUD)

  'turn turbo OFF at startup
  '  turbo := false   <--- motor's PWM duty * 50%
  '  turbo := true    <--- motor's PWM duty * 100%
  '
  '  turbo should be used sparingly.  It puts full power to the motors, which may damage them if sustained.   
  turbo[LEFT_MOTOR] := turbo[RIGHT_MOTOR] := false
  
  
  'execute main loop
  Fly_Yellow_Bee_FLYYYYYYYY


  
pub Fly_Yellow_Bee_FLYYYYYYYY | i, valid, l_motor, r_motor  

  'motors_enabled := true
  
  'main program loop
  repeat



    '================================================
    'get the start packet
    '================================================
    valid~~

    repeat until (i := xb.rxcheck) == "!"
    ifnot i == "!"
      valid~
    i := xb.rxtime(50)
    ifnot i == "Y"
      valid~
    i := xb.rxtime(50)
    ifnot i == "B"
      valid~                    'invalidate if an incorrect byte has been received

    '================================================
    'process packet
    '================================================
    
    if valid
      xb.str(STRING("PACKET VALIDATED",13))
      motors_enabled := true    'valid packet received, enable the motors
    else
      xb.str(STRING("PACKET inVALIDATED",13))
      motors_enabled := false   'not a valid packet, disable the motors
      next
          

    motor_power[LEFT_MOTOR] := xb.rxtime(50)            'get the speed value for the left motor
    motor_power[RIGHT_MOTOR] := xb.rxtime(50)           'get the speed value for the right motor
    turbo[LEFT_MOTOR] := turbo[RIGHT_MOTOR] := xb.rxtime(50)  'get turbo data from the controller. For now, both motors are turbo'd at the same time


    '================================================
    'stick data to system
    '================================================


    'actually stick the speed values to the motors    
    SetMotor(motor_power[LEFT_MOTOR], LEFT_MOTOR)       
    SetMotor(motor_power[RIGHT_MOTOR], RIGHT_MOTOR)     


      




{  
    repeat i from 0 to 100      'ramp left up
      SetMotor(i, LEFT_MOTOR)
      PauseMS(50)
    
    repeat i from 100 to 0      'ramp left down
      SetMotor(i, LEFT_MOTOR)
      PauseMS(50)

    repeat i from 0 to 100      'ramp right up
      SetMotor(i, RIGHT_MOTOR)
      PauseMS(50)
    
    repeat i from 100 to 0      'ramp right down
      SetMotor(i, RIGHT_MOTOR)
      PauseMS(50)

}

    'PauseMS(2500)


pub Apply_Pilot_Assists


  'start and stop engine safeties (dont forget the rainbow RGB LED). Hold down+A to enable engines, and down+B to disable engines


  'this one is going to deal alot with the duty[] array.

  if Landing_Assist
    'get reading from compass

    'max deviation from center = ? (the maximum change in motor power aloud to correct for when auto leveling)
    '

  
pri SetMotor(val, side)
  'sets the [side] motor's duty cycle value

  'null out any motor speed settings if motors are disabled
  'ifnot motors_enabled
  ' val := 0
  
  'FIXME math hack to get turbo to work with true or false
  motor[side].SetDuty((val&motors_enabled)>>(((turbo[side]-1)&1)))

pri PauseMS(Duration)
''  Pause execution for 'duration' milliseconds.
''  This routine is based on the set clock frequency.
''  
''  params:  Duration = number of milliseconds to delay                                                                                               
''  return:  none

  waitcnt(((clkfreq / 1_000 * Duration - 3932) #> 381) + cnt)